#ifdef SUPPORT_MS_EXTENSIONS
#pragma bss_seg(".inv_imu_apex.data.bss")
#pragma data_seg(".inv_imu_apex.data")
#pragma const_seg(".inv_imu_apex.text.const")
#pragma code_seg(".inv_imu_apex.text")
#endif
/*
 * ________________________________________________________________________________________________________
 * Copyright (c) 2017 InvenSense Inc. All rights reserved.
 *
 * This software, related documentation and any modifications thereto (collectively "Software") is subject
 * to InvenSense and its licensors' intellectual property rights under U.S. and international copyright
 * and other intellectual property rights laws.
 *
 * InvenSense and its licensors retain all intellectual property and proprietary rights in and to the Software
 * and any use, reproduction, disclosure or distribution of the Software without an express license agreement
 * from InvenSense is strictly prohibited.
 *
 * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, THE SOFTWARE IS
 * PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED
 * TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT.
 * EXCEPT AS OTHERWISE PROVIDED IN A LICENSE AGREEMENT BETWEEN THE PARTIES, IN NO EVENT SHALL
 * INVENSENSE BE LIABLE FOR ANY DIRECT, SPECIAL, INDIRECT, INCIDENTAL, OR CONSEQUENTIAL DAMAGES, OR ANY
 * DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT,
 * NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
 * OF THE SOFTWARE.
 * ________________________________________________________________________________________________________
 */

#include "inv_imu_defs.h"
#include "inv_imu_extfunc.h"
#include "inv_imu_driver.h"
#include "inv_imu_apex.h"

int inv_imu_apex_enable_ff(struct inv_imu_device *s)
{
    int status = 0;
    uint8_t value;

    status |= inv_imu_start_dmp(s);

    status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
    value &= ~APEX_CONFIG1_FF_ENABLE_MASK;
    value |= (uint8_t)APEX_CONFIG1_FF_ENABLE_EN;
    status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);

    return status;
}

int inv_imu_apex_disable_ff(struct inv_imu_device *s)
{
    int status = 0;
    uint8_t value;

    status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
    value &= ~APEX_CONFIG1_FF_ENABLE_MASK;
    value |= (uint8_t)APEX_CONFIG1_FF_ENABLE_DIS;
    status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);

    return status;
}

int inv_imu_apex_enable_smd(struct inv_imu_device *s)
{
    int status = 0;
    uint8_t value;

    status |= inv_imu_start_dmp(s);

    status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
    value &= ~APEX_CONFIG1_SMD_ENABLE_MASK;
    value |= (uint8_t)APEX_CONFIG1_SMD_ENABLE_EN;
    status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);

    return status;
}

int inv_imu_apex_disable_smd(struct inv_imu_device *s)
{
    int status = 0;
    uint8_t value;

    status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
    value &= ~APEX_CONFIG1_SMD_ENABLE_MASK;
    value |= (uint8_t)APEX_CONFIG1_SMD_ENABLE_DIS;
    status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);

    return status;
}

int inv_imu_apex_init_parameters_struct(struct inv_imu_device *s, inv_imu_apex_parameters_t *apex_inputs)
{
    int status = 0;
    (void)s;

    /* Default parameters at POR */
    apex_inputs->pedo_amp_th          = APEX_CONFIG3_PEDO_AMP_TH_62_MG;
    apex_inputs->pedo_step_cnt_th     = 0x5;
    apex_inputs->pedo_step_det_th     = 0x2;
    apex_inputs->pedo_sb_timer_th     = APEX_CONFIG4_PEDO_SB_TIMER_TH_150_SAMPLES;
    apex_inputs->pedo_hi_enrgy_th     = APEX_CONFIG4_PEDO_HI_ENRGY_TH_104_MG;
    apex_inputs->tilt_wait_time       = APEX_CONFIG5_TILT_WAIT_TIME_4_S;
    apex_inputs->power_save_time      = APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_8_S;
    apex_inputs->power_save           = APEX_CONFIG0_DMP_POWER_SAVE_EN;
    apex_inputs->sensitivity_mode     = APEX_CONFIG9_SENSITIVITY_MODE_NORMAL;
    apex_inputs->low_energy_amp_th    = APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_80_MG;
    apex_inputs->smd_sensitivity      = APEX_CONFIG9_SMD_SENSITIVITY_0;
    apex_inputs->ff_debounce_duration = APEX_CONFIG9_FF_DEBOUNCE_DURATION_2000_MS;
    apex_inputs->ff_max_duration_cm   = APEX_CONFIG12_FF_MAX_DURATION_204_CM;
    apex_inputs->ff_min_duration_cm   = APEX_CONFIG12_FF_MIN_DURATION_10_CM;
    apex_inputs->lowg_peak_th         = APEX_CONFIG10_LOWG_PEAK_TH_563_MG;
    apex_inputs->lowg_peak_hyst       = APEX_CONFIG5_LOWG_PEAK_TH_HYST_156_MG;
    apex_inputs->lowg_samples_th      = APEX_CONFIG10_LOWG_TIME_TH_1_SAMPLE;
    apex_inputs->highg_peak_th        = APEX_CONFIG11_HIGHG_PEAK_TH_2500_MG;
    apex_inputs->highg_peak_hyst      = APEX_CONFIG5_HIGHG_PEAK_TH_HYST_156_MG;
    apex_inputs->highg_samples_th     = APEX_CONFIG11_HIGHG_TIME_TH_1_SAMPLE;

    return status;
}

int inv_imu_apex_configure_parameters(struct inv_imu_device *s, const inv_imu_apex_parameters_t *apex_inputs)
{
    int status = 0;
    uint8_t data;
    uint8_t apexConfig[7];
    APEX_CONFIG1_PED_ENABLE_t pedo_state;
    APEX_CONFIG1_TILT_ENABLE_t tilt_state;
    APEX_CONFIG1_FF_ENABLE_t ff_state;
    APEX_CONFIG1_SMD_ENABLE_t smd_state;

    /* DMP cannot be configured if it is running, hence make sure all APEX algorithms are off */
    status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &data);
    pedo_state  = (APEX_CONFIG1_PED_ENABLE_t)(data & APEX_CONFIG1_PED_ENABLE_MASK);
    tilt_state  = (APEX_CONFIG1_TILT_ENABLE_t)(data & APEX_CONFIG1_TILT_ENABLE_MASK);
    ff_state  = (APEX_CONFIG1_FF_ENABLE_t)(data & APEX_CONFIG1_FF_ENABLE_MASK);
    smd_state  = (APEX_CONFIG1_SMD_ENABLE_t)(data & APEX_CONFIG1_SMD_ENABLE_MASK);
    if (pedo_state == APEX_CONFIG1_PED_ENABLE_EN) {
        return INV_ERROR;
    }
    if (tilt_state == APEX_CONFIG1_TILT_ENABLE_EN) {
        return INV_ERROR;
    }
    if (ff_state == APEX_CONFIG1_FF_ENABLE_EN) {
        return INV_ERROR;
    }
    if (smd_state == APEX_CONFIG1_SMD_ENABLE_EN) {
        return INV_ERROR;
    }


    status |= inv_imu_switch_on_mclk(s);

    /* Power Save mode and low energy amplitude threshold (for Pedometer in Slow Walk mode) */
    /* APEX_CONFIG2_MREG1 */
    apexConfig[0] = (uint8_t)apex_inputs->power_save_time
                    | (uint8_t)apex_inputs->low_energy_amp_th;

    /* Pedometer parameters */
    /* APEX_CONFIG3_MREG1 */
    apexConfig[1] = (uint8_t)apex_inputs->pedo_amp_th
                    | (apex_inputs->pedo_step_cnt_th & APEX_CONFIG3_PED_STEP_CNT_TH_SEL_MASK);

    /* APEX_CONFIG4_MREG1 */
    apexConfig[2] = ((apex_inputs->pedo_step_det_th << APEX_CONFIG4_PED_STEP_DET_TH_SEL_POS)
                     & APEX_CONFIG4_PED_STEP_DET_TH_SEL_MASK)
                    | (uint8_t)apex_inputs->pedo_sb_timer_th
                    | (uint8_t)apex_inputs->pedo_hi_enrgy_th;

    /* Tilt, Lowg and highg parameters */
    /* APEX_CONFIG5_MREG1 */
    apexConfig[3] = (uint8_t)apex_inputs->tilt_wait_time
                    | (uint8_t)apex_inputs->lowg_peak_hyst
                    | (uint8_t)apex_inputs->highg_peak_hyst;

    status |= inv_imu_write_reg(s, APEX_CONFIG2_MREG1, 4, &apexConfig[0]);


    /* APEX_CONFIG0 */
    status |= inv_imu_read_reg(s, APEX_CONFIG0, 1, &apexConfig[0]);
    apexConfig[0] &= ~APEX_CONFIG0_DMP_POWER_SAVE_EN_MASK;
    apexConfig[0] |= apex_inputs->power_save;
    status |= inv_imu_write_reg(s, APEX_CONFIG0, 1, &apexConfig[0]);

    /* free fall parameter, SMD parameter and parameters for Pedometer in Slow Walk mode */
    /* APEX_CONFIG9_MREG1 */
    apexConfig[0] = (uint8_t)apex_inputs->ff_debounce_duration
                    | (uint8_t)apex_inputs->smd_sensitivity
                    | (uint8_t)apex_inputs->sensitivity_mode;

    /* Lowg and highg parameters and free fall parameters */
    /* APEX_CONFIG10_MREG1 */
    apexConfig[1] = (uint8_t)apex_inputs->lowg_peak_th
                    | (uint8_t)apex_inputs->lowg_samples_th;

    /* APEX_CONFIG11_MREG1 */
    apexConfig[2] = (uint8_t)apex_inputs->highg_peak_th
                    | (uint8_t)apex_inputs->highg_samples_th;

    status |= inv_imu_write_reg(s, APEX_CONFIG9_MREG1, 3, &apexConfig[0]);


    /* APEX_CONFIG12_MREG1 */
    apexConfig[0] = (uint8_t)apex_inputs->ff_max_duration_cm
                    | (uint8_t)apex_inputs->ff_min_duration_cm;

    status |= inv_imu_write_reg(s, APEX_CONFIG12_MREG1, 1, &apexConfig[0]);

    status |= inv_imu_switch_off_mclk(s);

    return status;
}

int inv_imu_apex_get_parameters(struct inv_imu_device *s, inv_imu_apex_parameters_t *apex_params)
{
    int status = 0;
    uint8_t data[7];
    uint8_t value;

    status |= inv_imu_read_reg(s, APEX_CONFIG0, 1, &value);
    apex_params->power_save = (APEX_CONFIG0_DMP_POWER_SAVE_t)(value & APEX_CONFIG0_DMP_POWER_SAVE_EN_MASK);

    /* Access continuous config registers (CONFIG2-CONFIG11) */
    status |= inv_imu_read_reg(s, APEX_CONFIG2_MREG1, sizeof(data), &data[0]);

    /* Get params from apex_config2 : dmp_power_save_time and low_energy_amp_th */
    apex_params->power_save_time = (APEX_CONFIG2_DMP_POWER_SAVE_TIME_t)
                                   (data[0] & APEX_CONFIG2_DMP_POWER_SAVE_TIME_SEL_MASK);
    apex_params->low_energy_amp_th = (APEX_CONFIG2_LOW_ENERGY_AMP_TH_t)
                                     (data[0] & APEX_CONFIG2_LOW_ENERGY_AMP_TH_SEL_MASK);

    /* Get params from apex_config3 : pedo_amp_th and pedo_step_cnt_th */
    apex_params->pedo_amp_th = (APEX_CONFIG3_PEDO_AMP_TH_t)
                               (data[1] & APEX_CONFIG3_PED_AMP_TH_SEL_MASK);
    apex_params->pedo_step_cnt_th = (data[1] & APEX_CONFIG3_PED_STEP_CNT_TH_SEL_MASK)
                                    >> APEX_CONFIG3_PED_STEP_CNT_TH_SEL_POS;

    /* Get params from apex_config4 : pedo_step_det_th, pedo_sb_timer_th and pedo_hi_enrgy_th */
    apex_params->pedo_step_det_th = (data[2] & APEX_CONFIG4_PED_STEP_DET_TH_SEL_MASK)
                                    >> APEX_CONFIG4_PED_STEP_DET_TH_SEL_POS;
    apex_params->pedo_sb_timer_th = (APEX_CONFIG4_PEDO_SB_TIMER_TH_t)
                                    (data[2] & APEX_CONFIG4_PED_SB_TIMER_TH_SEL_MASK);
    apex_params->pedo_hi_enrgy_th = (APEX_CONFIG4_PEDO_HI_ENRGY_TH_t)
                                    (data[2] & APEX_CONFIG4_PED_HI_EN_TH_SEL_MASK);

    /* Get params from apex_config5 : tilt_wait_time, lowg_peak_hyst and highg_peak_hyst */
    apex_params->tilt_wait_time = (APEX_CONFIG5_TILT_WAIT_TIME_t)
                                  (data[3] & APEX_CONFIG5_TILT_WAIT_TIME_SEL_MASK);
    apex_params->lowg_peak_hyst = (APEX_CONFIG5_LOWG_PEAK_TH_HYST_t)
                                  (data[3] & APEX_CONFIG5_LOWG_PEAK_TH_HYST_SEL_MASK);
    apex_params->highg_peak_hyst = (APEX_CONFIG5_HIGHG_PEAK_TH_HYST_t)
                                   (data[3] & APEX_CONFIG5_HIGHG_PEAK_TH_HYST_SEL_MASK);

    /* Get params from apex_config9 : ff_debounce_duration, smd_sensitivity and sensitivity_mode */
    apex_params->ff_debounce_duration = (APEX_CONFIG9_FF_DEBOUNCE_DURATION_t)
                                        (data[4] & APEX_CONFIG9_FF_DEBOUNCE_DURATION_SEL_MASK);
    apex_params->smd_sensitivity = (APEX_CONFIG9_SMD_SENSITIVITY_t)
                                   (data[4] & APEX_CONFIG9_SMD_SENSITIVITY_SEL_MASK);
    apex_params->sensitivity_mode = (APEX_CONFIG9_SENSITIVITY_MODE_t)
                                    (data[4] & APEX_CONFIG9_SENSITIVITY_MODE_MASK);

    /* Get params from apex_config10 : lowg_peak_th and lowg_samples_th */
    apex_params->lowg_peak_th = (APEX_CONFIG10_LOWG_PEAK_TH_t)
                                (data[5] & APEX_CONFIG10_LOWG_PEAK_TH_SEL_MASK);
    apex_params->lowg_samples_th = (APEX_CONFIG10_LOWG_TIME_TH_SAMPLES_t)
                                   (data[5] & APEX_CONFIG10_LOWG_TIME_TH_SEL_MASK);

    /* Get params from apex_config11 : highg_peak_th and highg_samples_th */
    apex_params->highg_peak_th = (APEX_CONFIG11_HIGHG_PEAK_TH_t)
                                 (data[6] & APEX_CONFIG11_HIGHG_PEAK_TH_SEL_MASK);
    apex_params->highg_samples_th = (APEX_CONFIG11_HIGHG_TIME_TH_SAMPLES_t)
                                    (data[6] & APEX_CONFIG11_HIGHG_TIME_TH_SEL_MASK);

    /* Access apex reg 12 */
    status |= inv_imu_read_reg(s, APEX_CONFIG12_MREG1, 1, &data[0]);

    /* Get params from apex_config12 : ff_max_duration_cm and ff_min_duration_cm */
    apex_params->ff_max_duration_cm = (APEX_CONFIG12_FF_MAX_DURATION_t)
                                      (data[0] & APEX_CONFIG12_FF_MAX_DURATION_SEL_MASK);
    apex_params->ff_min_duration_cm = (APEX_CONFIG12_FF_MIN_DURATION_t)
                                      (data[0] & APEX_CONFIG12_FF_MIN_DURATION_SEL_MASK);

    return status;
}

int inv_imu_apex_set_frequency(struct inv_imu_device *s, const APEX_CONFIG1_DMP_ODR_t frequency)
{
    uint8_t value;
    int status = 0;

    status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);

    value &= ~APEX_CONFIG1_DMP_ODR_MASK;
    value |= frequency;

    status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);
    return status;
}

int inv_imu_apex_enable_pedometer(struct inv_imu_device *s)
{
    int status = 0;
    uint8_t value;

    status |= inv_imu_start_dmp(s);

    /* Enable Pedometer */
    status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
    value &= ~APEX_CONFIG1_PED_ENABLE_MASK;
    value |= (uint8_t)APEX_CONFIG1_PED_ENABLE_EN;
    status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);

    return status;
}

int inv_imu_apex_disable_pedometer(struct inv_imu_device *s)
{
    int status = 0;
    uint8_t value;

    /* Disable Pedometer */
    status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
    value &= ~APEX_CONFIG1_PED_ENABLE_MASK;
    value |= (uint8_t)APEX_CONFIG1_PED_ENABLE_DIS;
    status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);

    return status;
}

int inv_imu_apex_enable_tilt(struct inv_imu_device *s)
{
    int status = 0;
    uint8_t value;

    status |= inv_imu_start_dmp(s);

    /* Enable Tilt */
    status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
    value &= ~APEX_CONFIG1_TILT_ENABLE_MASK;
    value |= (uint8_t)APEX_CONFIG1_TILT_ENABLE_EN;
    status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);

    return status;
}

int inv_imu_apex_disable_tilt(struct inv_imu_device *s)
{
    int status = 0;
    uint8_t value;

    /* Disable Tilt */
    status |= inv_imu_read_reg(s, APEX_CONFIG1, 1, &value);
    value &= ~APEX_CONFIG1_TILT_ENABLE_MASK;
    value |= (uint8_t)APEX_CONFIG1_TILT_ENABLE_DIS;
    status |= inv_imu_write_reg(s, APEX_CONFIG1, 1, &value);

    return status;
}

int inv_imu_apex_get_data_activity(struct inv_imu_device *s, inv_imu_apex_step_activity_t *apex_activity)
{
    uint8_t data[4];
    int status = inv_imu_read_reg(s, APEX_DATA0, 4, data);

    apex_activity->step_cnt = data[1] << 8 | data[0];
    apex_activity->step_cadence = data[2];
    apex_activity->activity_class = data[3] & APEX_DATA3_ACTIVITY_CLASS_MASK;

    return status;
}

int inv_imu_apex_get_data_free_fall(struct inv_imu_device *s, uint16_t *freefall_duration)
{
    uint8_t data[2];
    int status = inv_imu_read_reg(s, APEX_DATA4, 2, &data[0]);

    *freefall_duration = (data[1] << 8) | data[0];

    return status;
}

